﻿using System;
using System.Linq;
using System.Collections.Generic;
using System.Text;

namespace RobotCtrl
{
    class TrackTurn : Track
    {
        private float anglei = 0;
        public TrackTurn(float angle, float speed, float acceleration)
        {
            this.anglei = angle;
            this.length = (float)((Constants.AxleLength * Math.PI) / 360 * angle);
            //this.length = Constants.WheelCircumference * (float)Math.PI * (angle / 360);
            this.nominalSpeed = Math.Abs(speed);
            this.acceleration = acceleration;
            if (length < 0)
            {
                //reverse = true;                 // Rückwärts fahren
                this.reverse = (Math.Sign(angle) ^ Math.Sign(speed)) != 0; // Rückwärts fahren
                this.length *= -1;              // invertieren
            }
        }

        public override void IncrementalStep(float timeInterval, float newVelocity, out float leftSpeed, out float rightSpeed)
        {
            if (this.reverse)
            {
                leftSpeed = newVelocity;
                rightSpeed = newVelocity;
            }
            else
            {
                leftSpeed = -newVelocity;
                rightSpeed = -newVelocity;
            }
            
            this.currentVelocity = newVelocity;
            this.DoStep(timeInterval);
        }

        public override string ToString()
        {
            return this.GetType().Name + ": " + this.anglei + " Grad";
        }
    }
}
